%%  Clear workspace & load userpath

clear all;
close all;
clc;

addpath(userpath_matlab);

%%  Method Parameters

rho             = 1;
poliBaseType    = '';
order           = 2;    % 1 or 2
samplesPerNode  = 4;
isUniform       = true;
parallelization = true;

domainDimension = [8, 8];
quantityOfNodes = [8, 8];
center  = [0.5*domainDimension(1); 0.5*domainDimension(2)];

if(isUniform)
    [xNodes hx hy] = UniformGrid2D(quantityOfNodes(1), quantityOfNodes(2),...
        domainDimension(1), domainDimension(2), center);
    [xSamples hxSamples hySamples] = UniformGrid2D(quantityOfNodes(1)*samplesPerNode,...
        quantityOfNodes(2)*samplesPerNode,...
        domainDimension(1)*0.95, domainDimension(2)*0.95, center);
else
    xNodes = NonUniformGrid2(2, quantityOfNodes(1), domainDimension(1));
    xSamples = NonUniformGrid2(2, quantityOfNodes(1)*samplesPerNode,...
        domainDimension(1));
end

%if(parallelization)
%    matlabpool open 8;
%end
%%  Generate Form Functions
tic
totalNodes = size(xNodes,1);
totalSamples = size(xSamples,1);
dimension = size(xSamples,2);
baseDimension = getPolibaseDimension(order);

samplesShapeFunction = zeros(totalSamples,totalNodes);
gradSamplesShapeFunction = zeros(totalSamples,totalNodes,dimension);

parfor indexSample = 1:size(xSamples,1)

    %   Inner loop initialization
    gradAlpha = zeros(baseDimension,dimension);
    gradN = zeros(totalNodes,dimension);
    weights = zeros(totalNodes,1);

    for indexNode=1:totalNodes
        distance = norm(xSamples(indexSample,:) - xNodes(indexNode,:));
        weights(indexNode) = gaussianWeight (distance , rho);
    end
    B = assemblyBMatrix( xSamples(indexSample,:), xNodes, order , weights , poliBaseType);
    A = assemblyMomentMatrix ( xSamples(indexSample,:), xNodes, weights , poliBaseType, order );
    p = poliBase(xSamples(indexSample,:), order, poliBaseType);
    [alpha L U] = solveAlpha (A,p);
    N = alpha * B';

    samplesShapeFunction(indexSample,:) = N;

    gradP       = gradPoliBase(xSamples(indexSample, : ), order, poliBaseType);
    gradWeights = gradGaussianWeight(xNodes, xSamples(indexSample, : ), rho);
    gradA       = assemblyGradMomentMatrix(xSamples(indexSample, : ), xNodes,...
                  gradWeights, poliBaseType, order);
    loadVectorGradAlpha = assemblyLoadVectorGradAlpha ...
                        (gradP(:,1), gradP (:,2),...
                        gradA(:,:,1), gradA(:,:,2), alpha );

    for d = 1 : dimension
        gradAlpha(:,d) = solveGradAlpha(L, U,loadVectorGradAlpha(:,d)');
    end
    
    gradB = assemblyGradBMatrix(xSamples(indexSample, : ), xNodes, order,...
        gradWeights, poliBaseType);

    for d = 1:dimension
        gradN(:,d) = gradAlpha(:,d)' * B' +  alpha * gradB(:,:,d)';
    end
    
    gradSamplesShapeFunction(indexSample,:,:) = gradN; 
end
toc

%%  Finish parallelization

if(parallelization)
    matlabpool close;
end

%%  Zero Order Consistency

ZeroCheck = 0;

for indexSample = 1:size(xSamples,1)
    ZeroCheck = ZeroCheck + sum(samplesShapeFunction(indexSample,:));
end

ZeroCheck - size(xSamples,1)

%%  First Order Consistency

FirstCheck = zeros(totalSamples,2);

for indexSample = 1:size(xSamples,1)
    FirstCheck(indexSample,:) = samplesShapeFunction(indexSample,:)*xNodes;
end

diffCheck = FirstCheck - xSamples;
max(max(diffCheck))

%%  Second Order Consistency

SecondCheck = zeros(totalSamples,2);

for indexSample = 1:size(xSamples,1)
    SecondCheck(indexSample,:) = samplesShapeFunction(indexSample,:)*xNodes.^2;
end

diffCheck = SecondCheck - xSamples.^2;
max(max(diffCheck))

%%  Plot the grid

scatter(xNodes(:,1),xNodes(:,2),'r','filled'); hold on;
scatter(xSamples(:,1),xSamples(:,2),'Marker','+');

%%  Plot the shape function at one node

node = 28;
subplot(2,1,1),normalPeoplePlot(xSamples(:,1), xSamples(:,2), samplesShapeFunction(:,node)); 
subplot(2,1,2),scatter(xNodes(:,1),xNodes(:,2),'b','filled'); hold on;
subplot(2,1,2),scatter(xNodes(node,1),xNodes(node,2),'r','filled');

%%  Plot the shape function gradient (X) at one node

node = 14;
subplot(2,1,1),normalPeoplePlot(xSamples(:,1), xSamples(:,2), gradSamplesShapeFunction(:,node,1));
subplot(2,1,2),scatter(xNodes(:,1),xNodes(:,2),'b','filled'); hold on;
subplot(2,1,2),scatter(xNodes(node,1),xNodes(node,2),'r','filled');

%%  Plot the shape function gradient (Y) at one node

node = 37;
subplot(2,1,1),normalPeoplePlot(xSamples(:,1), xSamples(:,2), gradSamplesShapeFunction(:,node,2));
subplot(2,1,2),scatter(xNodes(:,1),xNodes(:,2),'b','filled'); hold on;
subplot(2,1,2),scatter(xNodes(node,1),xNodes(node,2),'r','filled');

%%
node = 495;
normalPeoplePlot(xSamples(:,1), xSamples(:,2), samplesShapeFunction(:,node));